classdef QRIL_inverse
    methods ( Static )
        function Beta = QR_schmidt(A, T)
            [A_row, A_col] = size(A);
            max_rank = min(A_row, A_col);
            V = zeros(A_col, max_rank); 
            U = zeros(A_row, max_rank);
        
            precision = 1e-10;
            A_fro = norm(A, "fro");
            Error = A_fro^2; 
            Error_precision = 1e-16 * A_fro;

            % Find first non-zero vector
            s0 = 0;
             for s = 1:A_col
                u_vector = A(:, s);
                u_norm = norm(u_vector);
                if u_norm >= precision
                    s0 = s;
                    U(:, 1) = u_vector / u_norm;
                    break;
                end
            end
            
            if s0 == 0 % All columns are zero
                V = [];
                U = [];
                k = 1;
            else
                V(:, 1) = A' * U(:, 1);
                Error = Error - V(:, 1)' * V(:, 1);
            k = 2; 
                i = s0;
            end

            % Main orthogonalization loop
            while Error > Error_precision && k <= max_rank
                found = false;
                for s = i+1:A_col
                    U_temp = A(:, s);
                    U_temp = QRIL_inverse.MGS_Orth(U(:, 1:k-1), U_temp, k-1);
                    u_norm = norm(U_temp);
                    if u_norm >= precision
                        found = true;
                        break;
                    end
                end
                
                if ~found
                    break; % No valid vector found
                end
                
                U_temp = U_temp / u_norm;
                i = s;
                V_temp = A' * U_temp;
                Error = Error - V_temp' * V_temp;
                U(:, k) = U_temp;
                V(:, k) = V_temp;
                k = k + 1;
            end
            
            % Trim matrices to actual rank
            actual_rank = k - 1;
            U = U(:, 1:actual_rank);
            V = V(:, 1:actual_rank);
      
            % Solve using Schmidt and MLE
            [H, C] = QRIL_inverse.Schmidt(V');
            T_solve = U' * T;
            [~, T_cols] = size(T_solve);
            Beta = zeros(size(C, 2), T_cols);
            
            for col = 1:T_cols
                T_vec = T_solve(:, col);
                [Beta_col, ~] = QRIL_inverse.MLE_iter_solver(H, T_vec, 1000, 1e-10);
                Beta(:, col) = Beta_col;
            end
            Beta = C * Beta;
        end

        function Beta = directly_MLE_iter(A, T)
            [~, T_cols] = size(T);
            Beta = zeros(size(A, 2), T_cols);
            
            parfor col = 1:T_cols
                T_vec = T(:, col);
                [Beta_col, ~] = QRIL_inverse.MLE_iter_solver(A, T_vec, 1000, 1e-7);
                Beta(:, col) = Beta_col;
            end
        end

        function A_pinv = QR_inverse(A)
            [m, n] = size(A);
            max_rank = min(m, n);
            V = zeros(n, max_rank); 
            U = zeros(m, max_rank);
            
            precision = 1e-10;
            A_fro = norm(A, "fro");
            Error = A_fro^2;
            Error_precision = 1e-12 * A_fro;

            % Find first non-zero vector
            s0 = 0;
            for s = 1:n
                u_vector = A(:, s);
                u_norm = norm(u_vector);
                if u_norm >= precision
                    s0 = s;
                    U(:, 1) = u_vector / u_norm;
                    break;
                end
            end
            
            if s0 == 0 % All columns are zero
                A_pinv = zeros(n, m);
                return;
            else
                V(:, 1) = A' * U(:, 1);
                Error = Error - V(:, 1)' * V(:, 1);
            k = 2; 
                i = s0;
            end

            % Main orthogonalization loop
            while Error > Error_precision && k <= max_rank
                found = false;
                for s = i+1:n
                    U_temp = A(:, s);
                    U_temp = QRIL_inverse.MGS_Orth(U(:, 1:k-1), U_temp, k-1);
                    u_norm = norm(U_temp);
                    if u_norm >= precision
                        found = true;
                        break;
                    end
                end
                
                if ~found
                    break;
                end
                
                U_temp = U_temp / u_norm;
                i = s;
                V_temp = A' * U_temp;
                Error = Error - V_temp' * V_temp;
                U(:, k) = U_temp;
                V(:, k) = V_temp;
                k = k + 1;
            end
            
            % Trim matrices to actual rank
            actual_rank = k - 1;
            U = U(:, 1:actual_rank);
            V = V(:, 1:actual_rank);
            
            % Compute pseudoinverse
            V_pinv = QRIL_inverse.Schmidt_MLE_solver(V');
            A_pinv = V_pinv * U';
        end

        function Beta = Schmidt_MLE_solver(A,T)
            [A_row, A_col] = size(A);
            [~, T_cols] = size(T);
            [H, C] = QRIL_inverse.Schmidt(A);
            Beta = zeros(size(C, 2), T_cols);
            
            parfor col = 1:T_cols
                T_vec = T(:, col);
                [Beta_col, ~] = QRIL_inverse.MLE_iter_solver(H, T_vec, 1000, 1e-7);
                Beta(:, col) = Beta_col;
            end
            Beta = C * Beta;

        end

        function [Beta, errors] = MLE_iter_solver(H, T, max_loops, epsilon)
            [m, n] = size(H);
            e = T;
            errors = zeros(max_loops, 1);
            Beta = zeros(n, 1);

            for cnt = 1:max_loops
                for i = 1:n
                    % 按列进行提取
                    h = H(:, i);
                    norm_h_sq = h' * h;
                    if norm_h_sq < 1e-14
                        continue;
                    end
                    % 求解，内积除以向量2范数的平方
                    beta = dot(e, h) / norm_h_sq;
                    e = e - beta * h;
                    Beta(i) = Beta(i) + beta;
                end
                
                errors(cnt) = norm(H * Beta - T);
                if errors(cnt) < epsilon
                    errors = errors(1:cnt);
                    return;
                end
            end
        end

        function [B, C] = Schmidt(A)
            K = [A' * A, A'];
            [m, n] = size(K);
            C = eye(m);
            
            for i = 1:m-1
                if abs(K(i, i)) < 1e-10
                    continue; % Skip near-zero pivot
                end
                for j = i+1:m
                    const = -K(j, i) / K(i, i);
                    K(j, :) = K(j, :) + const * K(i, :);
                    C(j, :) = C(j, :) + const * C(i, :);
                end
            end

            B = K(:, m+1:n)';
            C = C';
        end

        function u_orth = MGS_Orth(U, u, sizeU)
            u_orth = u;
            for k = 1:sizeU
                proj = u_orth' * U(:, k);
                u_orth = u_orth - proj * U(:, k);
            end
        end

    end % methods

end % class